/* 
 * Copyright 2011 kOSMik.
 * This file is part of kOSMik.
 * 
 * kOSMik is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * kOSMik is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with kOSMik.  If not, see <http://www.gnu.org/licenses/>.
 *  
 * kOSMik:
 * Open Street Map - Editor.
 * http://code.google.com/p/swp-dv-ws2010-osm-2/
 */

package de.fuberlin.kosmik.filter;

import android.location.Location;

/**
 * TrackFilter implements an algorithm to detect unnecessary coordinates.
 * 
 * @author Andreas
 */
public class TrackFilter implements Filter {

	private static final double EARTH_RADIUS = 6371000; // in meters
	private static final int ONE_MILLION = 1000000;

	/**
	 * The last location that has been returned from the filter.
	 */
	private Location lastReturnedLocation;
	/**
	 * The last know location. Used for reference calculation.
	 */
	private Location lastKnownLocation;

	private long deltaTime;
	private double acceptableAngleDivergence;
	private double comparingAngle;
	private double deleteRadius;
	private boolean walkOnStraightLine;

	/**
	 * Constructor with parameters.
	 * 
	 * @param acceptableTimestampDifference this is difference between the
	 *            acquisition of two fixes that needs to pass, as the fixes are
	 *            considered different though they are the same.
	 * @param acceptableAngleDivergence 0 <= x <= 360; acceptable angle
	 *            difference while walking straight forward.
	 * @param heapRadius (in meters) Which coordinates around a point belong to
	 *            a point heap?
	 */
	public TrackFilter(long acceptableTimestampDifference, double acceptableAngleDivergence,
			double heapRadius) {

		this.deltaTime = acceptableTimestampDifference;
		this.acceptableAngleDivergence = acceptableAngleDivergence;
		this.deleteRadius = heapRadius;
		this.walkOnStraightLine = false;
	}

	/**
	 * Straight line detector. If you go straight forward, detectLine realizes
	 * it.
	 * 
	 * @param location the location to check against.
	 * @return true, if a straight line is detected. false, otherwise.
	 * 
	 */
	private boolean detectLine(Location location) {
		// Check for angle.
		double newAngle = getPointAngle(location, lastKnownLocation);

		if (Math.abs(comparingAngle - newAngle) < acceptableAngleDivergence) {
			return true;
		} else if (Math.abs(comparingAngle - newAngle) + acceptableAngleDivergence >= 360) {
			return true;
		}
		return false;
	}

	/**
	 * Point heap detector. If you don't move, detectPointHeap realizes it.
	 * 
	 * @param location the new location to check against.
	 * @return true, if Point heap detected. false, otherwise.
	 */
	private boolean detectPointHeap(Location location) {

		double distance = getCoordinateDistance(lastReturnedLocation, location);

		if (distance < deleteRadius) {
			return true;
		}
		return false;

	}

	/**
	 * GetPointAngle calculates the movement angle depend on the last position.
	 * 
	 * @param loc1 the first location.
	 * @param loc2 the second location.
	 * 
	 * @return An angle between 0° and 360° degrees.
	 */
	private double getPointAngle(Location loc1, Location loc2) {

		// Calculate the differences of longitudes and latitudes.
		double latDifference = ONE_MILLION * loc1.getLatitude() - ONE_MILLION
				* loc2.getLatitude();
		double lonDifference = ONE_MILLION * loc1.getLongitude() - ONE_MILLION
				* loc2.getLongitude();

		// Special case: Longitude difference is zero.
		if (lonDifference == 0) {
			if (latDifference > 0) {
				return 90.0;
			}
			if (latDifference < 0) {
				return 270.0;
			}
			return 0;
		}

		// Otherwise calculate the angle with arctan and the gradient.
		double gradient = Math.abs(latDifference) / Math.abs(lonDifference);
		double pointAngle = Math.toDegrees(Math.atan(gradient));

		// Now return the result depend on the direction.
		if ((lonDifference > 0) && (latDifference < 0)) {
			return 360 - pointAngle;
		}
		if ((lonDifference < 0) && (latDifference >= 0)) {
			return 180 - pointAngle;
		}
		if ((lonDifference < 0) && (latDifference < 0)) {
			return 180 + pointAngle;
		}
		return pointAngle;

	}

	/**
	 * Calculates the distance between the two locations using haversine's
	 * formula.
	 * 
	 * @param loc1 the first location.
	 * @param loc2 the second location.
	 * 
	 * @return The distance between the two locations in meters.
	 */
	public double getCoordinateDistance(Location loc1, Location loc2) {

		double fLat = loc1.getLatitude();
		double fLon = loc1.getLongitude();

		double sLat = loc2.getLatitude();
		double sLon = loc2.getLongitude();

		double latDifference = Math.toRadians(fLat - sLat);
		double lonDifference = Math.toRadians(fLon - sLon);

		double a = Math.sin(latDifference / 2) * Math.sin(latDifference / 2)
				+ Math.cos(Math.toRadians(sLat)) * Math.cos(Math.toRadians(fLat))
				* Math.sin(lonDifference / 2) * Math.sin(lonDifference / 2);

		double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));

		double distance = EARTH_RADIUS * c;

		return distance;
	}

	@Override
	public Location filter(Location location) {
		Location retVal = location;

		if (null == lastKnownLocation) {
			lastReturnedLocation = location;
			lastKnownLocation = location;
			retVal = location;
		}

		// Is there a point heap?
		else if (detectPointHeap(location)) {
			retVal = lastReturnedLocation;
		}

		// Is there a straight line?
		else if (detectLine(location)) {
			walkOnStraightLine = true;
			if (location.getTime() - lastReturnedLocation.getTime() < deltaTime) {
				retVal = lastReturnedLocation;
			}
		}

		// Maybe we reach a straight line end?
		else if (walkOnStraightLine) {
			walkOnStraightLine = false;
			retVal.set(lastKnownLocation);
		}

		if ((lastReturnedLocation.getLatitude() != retVal.getLatitude())
				&& (lastReturnedLocation.getLongitude() != retVal.getLongitude())) {
			comparingAngle = getPointAngle(location, lastKnownLocation);
		}
		lastReturnedLocation = retVal;
		lastKnownLocation = location;
		return retVal;

	}
}
